#include <ros/ros.h>
#include <std_msgs/String.h>
#include <mosquitto.h>
#include "rapidjson/document.h"
#include "rapidjson/writer.h"
#include "rapidjson/stringbuffer.h"
#include <iostream>
#include <thread>
#include <mutex>
#include <string>
#include <atomic>
#include <boost/bind.hpp>

// MQTT回调数据结构
struct MqttData {
    ros::Publisher command_pub;
    ros::Publisher control_pub;
    ros::Publisher mqtt_pub;
    std::mutex mqtt_mutex;
    std::atomic<bool> connected{false};
};

// 全局变量
int g_mqtt_qos = 0;

// 连接成功回调
void on_connect(struct mosquitto* mosq, void* userdata, int rc) {
    MqttData* data = static_cast<MqttData*>(userdata);
    
    if (rc == 0) {
        std::cout << "MQTT连接已建立" << std::endl;
        data->connected = true;
        
        // 订阅主题
        mosquitto_subscribe(mosq, nullptr, "vehicle/command", 0);
        mosquitto_subscribe(mosq, nullptr, "vehicle/control", 0);
        std::cout << "已订阅MQTT主题: vehicle/command 和 vehicle/control" << std::endl;
    } else {
        std::cout << "MQTT连接失败,错误码:" << rc << std::endl;

    }
}

// 连接断开回调
void on_disconnect(struct mosquitto* mosq, void* userdata, int rc) {
    MqttData* data = static_cast<MqttData*>(userdata);
    data->connected = false;
    std::cout << "MQTT连接已断开,错误码:" << rc << ",尝试重连..."<< std::endl;

}

// 消息到达回调
void on_message(struct mosquitto* mosq, void* userdata, const struct mosquitto_message* message) {
    MqttData* data = static_cast<MqttData*>(userdata);
    
    std::string topic = message->topic;
    std::string payload(static_cast<const char*>(message->payload), message->payloadlen);

    std::cout << "收到MQTT消息 - 主题:" << topic.c_str() << ",QoS:" << message->qos << ",内容:"<< payload.c_str() << std::endl;

    // 创建ROS消息
    std_msgs::String ros_msg;
    ros_msg.data = payload;

    // 根据主题发布到ROS话题
    if (topic == "vehicle/command") {
        data->command_pub.publish(ros_msg);
    } else if (topic == "vehicle/control") {
        data->control_pub.publish(ros_msg);
    }
}

// 创建测试JSON消息
std::string createTestMessage() {
    using namespace rapidjson;
    
    Document test_msg(kObjectType);
    Document::AllocatorType& allocator = test_msg.GetAllocator();
    
    test_msg.AddMember("id", "V001", allocator);
    test_msg.AddMember("speed", 45, allocator);
    test_msg.AddMember("battery", 78, allocator);
    test_msg.AddMember("mode", 1, allocator);
    test_msg.AddMember("timestamp", static_cast<int>(ros::Time::now().toSec()), allocator);

    StringBuffer buffer;
    Writer<StringBuffer> writer(buffer);
    test_msg.Accept(writer);
    
    return buffer.GetString();
}

// MQTT线程函数
void mqtt_thread(struct mosquitto* mosq, MqttData* data, const std::string& broker, int port, int keepalive) {
    int rc;
    
    // 连接到MQTT代理
    while (ros::ok()) {
        if (!data->connected) {
            try {
                rc = mosquitto_connect(mosq, broker.c_str(), port, keepalive);
                if (rc != MOSQ_ERR_SUCCESS) {
                    std::cout << "MQTT连接失败,错误码:" << rc << "," << mosquitto_strerror(rc)  << std::endl;
                    ros::Duration(2).sleep();  // 等待2秒后重试
                    continue;
                }
                
                // 启动网络循环
                rc = mosquitto_loop_start(mosq);
                if (rc != MOSQ_ERR_SUCCESS) {
                    std::cout << "无法启动MQTT网络循环,错误码:" << rc << std::endl;
                    mosquitto_disconnect(mosq);
                    ros::Duration(2).sleep();
                    continue;
                }
                
                std::cout << "MQTT网络循环已启动"<< std::endl;
                
            } catch (const std::exception& e) {
                std::cout << "MQTT线程异常:"<< e.what() << std::endl;
                ros::Duration(2).sleep();
            }
        }
        
        ros::Duration(0.1).sleep();
    }
    
    // 清理资源
    mosquitto_loop_stop(mosq, true);
    mosquitto_disconnect(mosq);
}

// ROS到MQTT的消息回调函数
void ros_to_mqtt_callback(const std_msgs::String::ConstPtr& msg, struct mosquitto* mosq, MqttData* data) {
    std::lock_guard<std::mutex> lock(data->mqtt_mutex);
    
    if (data->connected) {
        int rc = mosquitto_publish(mosq, nullptr, "test/topic", msg->data.length(), 
                                  msg->data.c_str(), g_mqtt_qos, false);
        if (rc != MOSQ_ERR_SUCCESS) {
            std::cout << "发布MQTT消息失败,错误码:"<< rc << std::endl;
        } else {
            std::cout << "已发布消息到MQTT主题: test/topic"<< std::endl;
        }
    } else {
        std::cout << "MQTT未连接,无法发布消息"<< std::endl;

    }
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "mqtt_bridge_node");
    ros::NodeHandle nh;
    ros::NodeHandle private_nh("~");

    // 初始化libmosquitto
    mosquitto_lib_init();

    // 读取MQTT配置参数，支持从命令行和参数服务器获取
    std::string mqtt_broker = "localhost";
    std::string mqtt_client_id = "ros_mqtt_bridge";
    std::string mqtt_username = "";
    std::string mqtt_password = "";
    int mqtt_port = 1883;
    int mqtt_keepalive = 60;
    bool enable_test_message = false;
    
    // 从命令行参数获取配置（如果提供）
    for (int i = 1; i < argc; i += 2) {
        if (i + 1 < argc) {
            std::string arg = argv[i];
            std::string value = argv[i + 1];
            
            if (arg == "--broker") mqtt_broker = value;
            else if (arg == "--client_id") mqtt_client_id = value;
            else if (arg == "--username") mqtt_username = value;
            else if (arg == "--password") mqtt_password = value;
            else if (arg == "--port") mqtt_port = std::stoi(value);
            else if (arg == "--qos") g_mqtt_qos = std::stoi(value);
            else if (arg == "--keepalive") mqtt_keepalive = std::stoi(value);
            else if (arg == "--enable_test_message") enable_test_message = (value == "true");
        }
    }
    
    // 从参数服务器获取配置（覆盖命令行参数）
    if (private_nh.hasParam("broker")) private_nh.getParam("broker", mqtt_broker);
    if (private_nh.hasParam("client_id")) private_nh.getParam("client_id", mqtt_client_id);
    if (private_nh.hasParam("username")) private_nh.getParam("username", mqtt_username);
    if (private_nh.hasParam("password")) private_nh.getParam("password", mqtt_password);
    if (private_nh.hasParam("port")) private_nh.getParam("port", mqtt_port);
    if (private_nh.hasParam("qos")) private_nh.getParam("qos", g_mqtt_qos);
    if (private_nh.hasParam("keepalive")) private_nh.getParam("keepalive", mqtt_keepalive);
    if (private_nh.hasParam("enable_test_message")) private_nh.getParam("enable_test_message", enable_test_message);
    
    // 打印使用的配置
    std::cout << "使用的MQTT配置:" << std::endl;
    std::cout << "  代理地址: " << mqtt_broker << std::endl;
    std::cout << "  客户端ID: " << mqtt_client_id << std::endl;
    std::cout << "  端口: " << mqtt_port << std::endl;
    std::cout << "  QoS: " << g_mqtt_qos << std::endl;
    std::cout << "  保持连接时间: " << mqtt_keepalive << "秒" << std::endl;
    std::cout << "  发送测试消息: " << (enable_test_message ? "是" : "否") << std::endl;
    
    // 参数验证
    if (mqtt_broker.empty()) {
        std::cout << "MQTT代理地址不能为空:" << std::endl;
        return 1;
    }
    
    if (mqtt_client_id.empty()) {
        std::cout << "MQTT客户端ID不能为空:" << std::endl;
        return 1;
    }
    
    if (mqtt_port <= 0 || mqtt_port > 65535) {
        std::cout << "MQTT端口号无效:" << std::endl;
        return 1;
    }
    
    if (g_mqtt_qos < 0 || g_mqtt_qos > 2) {
        std::cout << "MQTT QoS级别必须是0、1或2:" << std::endl;
        return 1;
    }
    
    if (mqtt_keepalive <= 0) {
        std::cout << "MQTT保持连接时间必须大于0:" << std::endl;
        return 1;
    }

    // 创建ROS发布者
    ros::Publisher command_pub = nh.advertise<std_msgs::String>("vehicle_command", 10);
    ros::Publisher control_pub = nh.advertise<std_msgs::String>("vehicle_control", 10);
    ros::Publisher mqtt_pub = nh.advertise<std_msgs::String>("mqtt_output", 10);

    // 创建MQTT数据结构
    MqttData mqtt_data;
    mqtt_data.command_pub = command_pub;
    mqtt_data.control_pub = control_pub;
    mqtt_data.mqtt_pub = mqtt_pub;

    // 创建MQTT客户端实例
    struct mosquitto* mosq = mosquitto_new(mqtt_client_id.c_str(), true, &mqtt_data);
    if (!mosq) {
        std::cout << "无法创建MQTT客户端实例,内存不足" << std::endl;
        return 1;
    }

    // 设置回调函数
    mosquitto_connect_callback_set(mosq, on_connect);
    mosquitto_disconnect_callback_set(mosq, on_disconnect);
    mosquitto_message_callback_set(mosq, on_message);

    // 设置用户名和密码（如果有）
    if (!mqtt_username.empty()) {
        int rc = mosquitto_username_pw_set(mosq, mqtt_username.c_str(), mqtt_password.c_str());
        if (rc != MOSQ_ERR_SUCCESS) {
            std::cout << "无法设置MQTT用户名/密码，错误码:"<< rc << std::endl;
        }
    }

    // 设置遗嘱消息
    std::string will_topic = "status/" + mqtt_client_id;
    std::string will_message = "offline";
    mosquitto_will_set(mosq, will_topic.c_str(), will_message.length(), will_message.c_str(), 1, false);

    // 创建ROS到MQTT的订阅者
    ros::Subscriber ros_to_mqtt_sub = nh.subscribe<std_msgs::String>(
        "ros_to_mqtt", 
        10, 
        boost::bind(ros_to_mqtt_callback, _1, mosq, &mqtt_data)
    );

    // 启动MQTT线程
    std::thread mqtt_thread_obj(mqtt_thread, mosq, &mqtt_data, mqtt_broker, mqtt_port, mqtt_keepalive);

    // 发送测试消息（可选）
    if (enable_test_message) {
        ros::Duration(3).sleep();  // 等待MQTT连接建立
        
        if (mqtt_data.connected) {
            std::string json_string = createTestMessage();
            int rc = mosquitto_publish(mosq, nullptr, "test/topic", json_string.length(), 
                                      json_string.c_str(), g_mqtt_qos, false);
            if (rc == MOSQ_ERR_SUCCESS) {
                std::cout << "已发送MQTT测试消息到主题: test/topic" << std::endl;

            } else {
                std::cout << "发送测试消息失败，错误码:"<< rc << std::endl;

            }
        } else {
                std::cout << "MQTT未连接,无法发送测试消息" << std::endl;
        }
    }

    // 进入ROS主循环
    std::cout << "MQTT桥接节点已启动,按Ctrl+C退出" << std::endl;
    ros::spin();

    // 清理资源
    mqtt_data.connected = false;
    if (mqtt_thread_obj.joinable()) {
        mqtt_thread_obj.join();
    }
    
    mosquitto_destroy(mosq);
    mosquitto_lib_cleanup();
    std::cout << "MQTT桥接节点已退出" << std::endl;
    return 0;
}
